#include "movebase.h"

#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <webots/motor.h>
#include <webots/robot.h>

#include "gait.h"

//*****************      m/s          m/s         rad/s
void Output_Wheel(float vel_x, float vel_y, float w_z) {
  float leg_vel[4];

  leg_vel[0] = vel_y - w_z * LEG_BASE_R / cos(X_R_ANGLE);
  leg_vel[1] = vel_y - w_z * LEG_BASE_R / cos(X_R_ANGLE);
  leg_vel[2] = vel_y + w_z * LEG_BASE_R / cos(X_R_ANGLE);
  leg_vel[3] = vel_y + w_z * LEG_BASE_R / cos(X_R_ANGLE);

  gait_params[GAIT_STATE].leg0.step_length =
      100 * leg_vel[0] / gait_params[GAIT_STATE].leg0.freq;
  gait_params[GAIT_STATE].leg1.step_length =
      100 * leg_vel[1] / gait_params[GAIT_STATE].leg1.freq;
  gait_params[GAIT_STATE].leg2.step_length =
      100 * leg_vel[2] / gait_params[GAIT_STATE].leg2.freq;
  gait_params[GAIT_STATE].leg3.step_length =
      100 * leg_vel[3] / gait_params[GAIT_STATE].leg3.freq;

  printf("\r\n[0]=%f [1]=%f  [2]=%f  [3]=%f", leg_vel[0], leg_vel[1],
         leg_vel[2], leg_vel[3]);
}
